#include <FrameAllocator.h>
#include <string.h>

dword FrameAllocator::bitmap[FRAMES_MAX / 32];
Spinlock FrameAllocator::Lock;

void FrameAllocator::Init() {
	/* Mark whole memory as used */
	memset(bitmap, 0xFF, sizeof(bitmap));
}

dword FrameAllocator::Alloc(dword start, unsigned count) {
	dword c, frame, i;
	Lock.Lock();
	while (1) {
		frame = FirstFree(start);
		if (!frame) {
			Lock.Unlock();
			return 0;
		}
		c = count;
		i = frame;

		while (c) {
			if (test(i))
				break;
			c--;
			i += 0x1000;
		}

		if (!c) {
			for (i = 0; i < count; i++)
				set(i * 0x1000 + frame);
			Lock.Unlock();
			return frame;
		}
	}

	Lock.Unlock();
	return 0;
}

void FrameAllocator::Free(dword start, unsigned frames) {
	Lock.Lock();
	for (dword i = 0; i < frames; i++)
		clear(start + i * 0x1000);
	Lock.Unlock();
}

void FrameAllocator::MarkFrames(dword start, dword frames, bool used) {
	for (dword i = 0; i < frames; i++) {
		if (used)
			set(start + i * 0x1000);
		else
			clear(start + i * 0x1000);
	}
}

void FrameAllocator::set(dword frame) {
	frame /= 0x1000;
	dword idx = INDEX_FROM_BIT(frame);
	dword off = OFFSET_FROM_BIT(frame);
	bitmap[idx] |= (0x1 << off);
}

void FrameAllocator::clear(dword frame) {
	frame /= 0x1000;
	dword idx = INDEX_FROM_BIT(frame);
	dword off = OFFSET_FROM_BIT(frame);
	bitmap[idx] &= ~(0x1 << off);
}

bool FrameAllocator::test(dword frame) {
	frame /= 0x1000;
	dword idx = INDEX_FROM_BIT(frame);
	dword off = OFFSET_FROM_BIT(frame);
	return (bitmap[idx] & (0x1 << off));
}

dword FrameAllocator::FirstFree(dword start) {
	start /= 0x1000;

	for (dword i = INDEX_FROM_BIT(start), j = OFFSET_FROM_BIT(start); i < FRAMES_MAX / 32; i++, j = 0) {
		if (bitmap[i] != 0xFFFFFFFF) {
			/* at least one bit is free here */
			for (; j < 32; j++) {
				dword to_test = 0x1 << j;
				if (!(bitmap[i] & to_test)) {
					return (i * 32 + j) * 0x1000;
				}
			}
		}
	}

	return 0;
}

